/*
 *
 * Copyright (C) 2014
 * Julio Jarquin, Gustavo Arechavaleta <garechav@cinvestav.edu.mx>
 * CINVESTAV - Saltillo Campus
 *
 * This file is part of HRLocomotion-1.0
 * HRLocomotion-1.0 free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * HRLocomotion-1.0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301  USA
 */

/**
 *	\file src/mpc_constraints.cc
 *	\author Julio Jarquin, Gustavo Arechavaleta.
 *	\version 1.0
 *	\date 2015
 *
 *	Implementation of the MPCConstraints class.
 */

#include "HRLocomotion/mpc_constraints.h"
namespace hrLocomotion {
/*
MPCConstraints::MPCConstraints(): kMaxStepX (0.08), kMinStepX (-0.04), kMaxStepY (0.160), kMinFootSeparation ( 0.09)   //TODO: default constructor needs to be modified according to every robot
{
}
*/
MPCConstraints::MPCConstraints(MPCData &mpcData): internalMPCData(mpcData), kMaxStepX (0.08), kMinStepX (-0.04), kMaxStepY (0.160), kMinFootSeparation ( 0.09){


}

void MPCConstraints::mpcTest(){
    std::cout << "Stage: " << internalMPCData.stage << std::endl;
}

void MPCConstraints::init(const MPCData & mpcData){

    using std::sqrt;        //This need to be changed to a more general form.
    using std::pow;         //Same here

    ////TODO Stack------------------already....
    int N = mpcData.N_horizon;
    int m = mpcData.m_steps;

    /** feet constraints */

    A_fpos.setZero(5,2*(N+m));  //TODO: a more efficient way to do this init.
    bup_fpos.resize(5);                                       //TODO: a more efficient way to do this init.

    //Set the size of the foot constraint matrices
    A_lfparams.resize(5,2);
    A_rfparams.resize(5,2);

    //Foot step position parameters with Left foot  on the ground
    A_lfparams <<   0,      1,
                  100,    -14.2857,
                   25,    -50,
                  -25,    -50,
                 -100,    -14.2857;

    //Foot step position parameters with Right foot  on the ground
    A_rfparams <<   0,     -1,
                  100,     14.2857,
                   25,     50,
                  -25,     50,
                 -100,    14.2857;


    //Set the size of the parameters of the feet
    bup_fparams.resize(5);
    bbox_lfparams.resize(2,4);
    bbox_rfparams.resize(2,4);

    //Set the parameters value;
    bup_fparams << -0.09, 5.2857, 8, 8, 5.2857;
    bbox_lfparams << 0.11,0.11,-0.047,-0.047,-0.05,0.038,0.038,-0.05;
    bbox_rfparams << 0.11,0.11,-0.047,-0.047,-0.038,0.050,0.050,-0.038;


    /** ZMP constraints */
    bup_zmp.resize(4*N); //Init vector

    zmp_margin = sqrt((pow((0.038*.9), 2))/2); //ZMP circle margin   ///TODO: add a cast in case of real_t:float
    Diag_zmp.resize(4*N,2*N);                  //Diagonal ZMP normal vectors matrix
    Diag_zmp << MatrixXr::Zero(N,N), MatrixXr::Identity(N,N)*(-1),
                MatrixXr::Identity(N,N), MatrixXr::Zero(N,N),
                MatrixXr::Zero(N,N), MatrixXr::Identity(N,N),
                MatrixXr::Identity(N,N)*(-1), MatrixXr::Zero(N,N);
    bup_zparams = bup_zparams.setOnes(4*N)*zmp_margin;

    MatrixXr A_zmp_aux;
    A_zmp_aux.resize(2*N,2*(N+m));
    for(int i = 0; i != mpcData.s_stage ; i++){
        A_zmp_aux << mpcData.Pzu, -mpcData.Ur[i], MatrixXr::Zero(N,N+m), MatrixXr::Zero(N,N+m), mpcData.Pzu, -mpcData.Ur[i];
        A_zmp.push_back(Diag_zmp*A_zmp_aux);
    }


    ///Set A and b size
    A_constraints.resize(A_zmp[0].rows()+A_fpos.rows(),2*(N+m));
    bup_constraints.resize(bup_zmp.rows()+bup_fpos.rows());
    //b_lwconstraints should return a null pointer as there are no lower constraints to be consider.


}

    //Heavy code Part goes here
    //Initializes the constraint matrix A_ and the bound vectors b_lw and b_up
void MPCConstraints::setConstraints(const MPCData & mpcData, const FootStatus & footStatus, const Vector3r & footPosition, RobotState & robotState){
    Vector2r footPosition_xy ;


    /** feet constraints */

    footPosition_xy << footPosition(0), footPosition(1);

    if(footStatus == hrLocomotion::kLeftFootOnGround){
        bup_fpos = bup_fparams + A_lfparams*footPosition_xy ;
        A_fpos.col(mpcData.N_horizon) = A_lfparams.col(0);
        A_fpos.col(2*mpcData.N_horizon + mpcData.m_steps) = A_lfparams.col(1);
    }

    else if(footStatus == hrLocomotion::kRightFootOnGround){
        bup_fpos = bup_fparams + A_rfparams*footPosition_xy ;
        A_fpos.col(mpcData.N_horizon) = A_rfparams.col(0);
        A_fpos.col(2*mpcData.N_horizon + mpcData.m_steps) = A_rfparams.col(1);
    }
    else if(footStatus == hrLocomotion::kBoothFeetOnGround){

    }
    else{

    }

    /** ZMP constraints */
    VectorXr xfyf(2*mpcData.N_horizon);
    //MatrixXr mat_01 = mpcData.Uc[mpcData.stage]*footPosition(0) - mpcData.Pzs*robotState.getX();
    //MatrixXr mat_02 = mpcData.Uc[mpcData.stage]*footPosition(1) - mpcData.Pzs*robotState.getY();
    xfyf << (mpcData.Uc[mpcData.stage]*footPosition(0) - mpcData.Pzs*robotState.getX()), (mpcData.Uc[mpcData.stage]*footPosition(1) - mpcData.Pzs*robotState.getY());
    bup_zmp = bup_zparams + Diag_zmp*xfyf;

    A_constraints << A_fpos, A_zmp[mpcData.stage];
    bup_constraints << bup_fpos, bup_zmp;
    //blw_constraints  has to be a null pointer

    assert( blw_constraints.data() == NULL);


}


}
